/*
 * pid.c
 *
 *  Created on: 2021年10月29日
 *      
 */
#include "pid.h"
#include "sys.h"
void PositionPidRealize(PID* vPID);
void IncrementPidRealize(PID* vPID);
void ExpertPidRealize(PID* vPID);

//////////////////////////////////////////////////
void PidInit_X(PID* vPID)
{
	vPID->setPoint = 0.00f;
	vPID->actualPoint = 0.00f;
	vPID->preError = 0.00f;
	vPID->pre2Error = 0.00f;
	vPID->integral = 0.00f;
	vPID->KP = 5.0f;
	vPID->KI = 0.01f;
	vPID->KD = 0.0f;
	vPID->result = 0.0f;
	vPID->maximum = 9500;
	vPID->minimum = 0.00;
	vPID->errorabsmax = 200;
	vPID->errorabsmid = 50.0;
	vPID->errorabsmin = -200;
}


void PidInit_Y(PID* vPID)
{
	vPID->setPoint = 0.00f;
	vPID->actualPoint = 0.00f;
	vPID->preError = 0.00f;
	vPID->pre2Error = 0.00f;
	vPID->integral = 0.00f;
	vPID->KP = 5.0f;
	vPID->KI = 0.01f;
	vPID->KD = 0.0f;
	vPID->result = 0.0f;
	vPID->maximum = 9500;
	vPID->minimum = -9500;
	vPID->errorabsmax = 1550;
	vPID->errorabsmid = 50.0;
	vPID->errorabsmin = 1.0;
}
/////////////////////////////////////////////////////
static float myfabs(float data)
{
	return data >= 0 ? data:(-data);
}
static float mylimit(PID* vPID)
{
	return vPID->result >= vPID->maximum ? vPID->maximum : vPID->result <= vPID->minimum ? vPID->minimum : vPID->result;
}

void PidInit(PID* vPID)
{
	vPID->setPoint = 0.00f;
	vPID->actualPoint = 0.00f;
	vPID->preError = 0.00f;
	vPID->pre2Error = 0.00f;
	vPID->integral = 0.00f;
	vPID->KP = 5.0f;
	vPID->KI = 0.01f;
	vPID->KD = 0.0f;
	vPID->result = 0.0f;
	vPID->maximum = 9500;
	vPID->minimum = 0.00;
	vPID->errorabsmax = 1550;
	vPID->errorabsmid = 50.0;
	vPID->errorabsmin = 1.0;
}

void PidDeInit(PID* vPID)
{
	vPID->setPoint = 0.00f;
	vPID->actualPoint = 0.00f;
	vPID->preError = 0.00f;
	vPID->pre2Error = 0.00f;
	vPID->integral = 0.00f;
	vPID->KP = 5.0f;
	vPID->KI = 0.01f;
	vPID->KD = 0.0f;
	vPID->result = 0.0f;
	vPID->maximum = 9500;
	vPID->minimum = 0.00;
	vPID->errorabsmax = 1550;
	vPID->errorabsmid = 50.0;
	vPID->errorabsmin = 1.0;
}

void PidRealize(PID* vPID,u8 PIDREALIZE_SELSECT)
{
	switch(PIDREALIZE_SELSECT)
	{
	case POSITIONPID:
		PositionPidRealize(vPID);
		break;
	case INCREATEMENTPID:
		IncrementPidRealize(vPID);
		break;
	case EXPERTPID:
		ExpertPidRealize(vPID);
		break;
	}
}

//函数名称：位置型PID实现
void PositionPidRealize(PID* vPID)
{
	float err;
	float result;
	err = vPID->setPoint - vPID->actualPoint;
	vPID->integral += err;
	result = vPID->KP * err + vPID->KI * vPID->integral + vPID->KD * (err - vPID->preError);
	vPID->preError = err;
	vPID->result = result;
}

//函数名称：增量型PID实现
void IncrementPidRealize(PID* vPID)
{
	float err,increment;
	err = vPID->setPoint - vPID->actualPoint;
	increment = vPID->KP * (err - vPID->preError) + vPID->KI * err + vPID->KD * (err - 2 * vPID->preError + vPID->pre2Error);
	vPID->pre2Error = vPID->preError;
	vPID->preError = err;
	vPID->result = vPID->result + increment;
	vPID->result = mylimit(vPID);
}


void ExpertPidRealize(PID* vPID)
{
	float thiserror;            //本次误差
	float deltaerror;           //本次误差偏差
	float lastdeltaerror;       //上一次误差偏差
	float result;               //本次调节输出值

	thiserror=vPID->setPoint-vPID->actualPoint;      //计算本次误差
	deltaerror=thiserror-vPID->preError;          //计算本次误差偏差
	lastdeltaerror=vPID->preError-vPID->pre2Error;//计算上次误差偏差

	//（1）判断本次误差是否查过误差极限最大值
	if(myfabs(thiserror)>=vPID->errorabsmax)
	{    /*执行规则1*/
		if(thiserror>0)
		{
			result=vPID->maximum;
		}
		if(thiserror<0)
		{
			result=vPID->minimum;
		}
	}

	if((thiserror*deltaerror>0)||(deltaerror==0))
	{/*执行规则2*/
		if(myfabs(thiserror)>=vPID->errorabsmid)
		{
			result=vPID->result+2.0*(vPID->KP*deltaerror+vPID->KI*thiserror+vPID->KD*(deltaerror-lastdeltaerror));
		}
		else
		{
			result=vPID->result+0.4*(vPID->KP*deltaerror+vPID->KI*thiserror+vPID->KD*(deltaerror-lastdeltaerror));
		}
	}

	if(((thiserror*deltaerror<0)&&(deltaerror*lastdeltaerror>0))||(thiserror==0))
	{/*执行规则3*/
		result=vPID->result;
	}

	if((thiserror*deltaerror<0)&&(deltaerror*lastdeltaerror<0))
	{/*执行规则4*/
		if(myfabs(thiserror)>=vPID->errorabsmid)
		{
			result=vPID->result+2.0*vPID->KP*thiserror;
		}
		else
		{
			result=vPID->result+0.6*vPID->KP*thiserror;
		}
	}

	if((myfabs(thiserror)<=vPID->errorabsmin)&&(myfabs(thiserror)>0))
	{   /*执行规则5*/
		result=vPID->result+0.5*vPID->KP*deltaerror+0.3*vPID->KI*thiserror;
	}

	/*对输出限值，避免超调*/
	if(result>=vPID->maximum)
	{
		result=vPID->maximum;
	}
	if(result<=vPID->minimum)
	{
		result=vPID->minimum;
	}

	vPID->result=result;
	vPID->pre2Error=vPID->preError;
	vPID->preError=thiserror;
}



